#include <termios.h>
#include "ros/ros.h"
#include "config.h"
#include "std_msgs/String.h" 
#include <signal.h>
#include <boost/thread/thread.hpp>  
#include "ros_modbus.h"
#include <geometry_msgs/Twist.h>  

#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/poll.h>
#include <ros/ros.h>

#define KEYCODE_1 0x31  
#define KEYCODE_2 0x32  
#define KEYCODE_3 0x33 
#define KEYCODE_4 0x34 

int kfd = 0;
struct termios cooked, raw;
bool done;

//test send value
extern void writeModbus_in(unsigned char addr,unsigned int reg,unsigned int value);
//test receive value

class SmartCarKeyboardTeleopNode
{
    private:
        double walk_vel_;
        double run_vel_;
        double yaw_rate_;
        double yaw_rate_run_;
        
        geometry_msgs::Twist cmdvel_;
        ros::NodeHandle n_;
        ros::Publisher pub_;

    public:
        ~SmartCarKeyboardTeleopNode() { }
        void keyboardLoop();
};

SmartCarKeyboardTeleopNode* tbk;

int main(int agrc,char **argv)
{
    ros::init(agrc,argv,"modbus_node");
    ros::NodeHandle nh;

    SmartCarKeyboardTeleopNode tbk;
   
	/* 创建一个新的线程 */
    boost::thread t = boost::thread(boost::bind(&SmartCarKeyboardTeleopNode::keyboardLoop, &tbk));
    ros::spin();



    ros::Rate loop_rate(10);
    
    //串口初始化
    serialInit();

    return 0;
}

/**
  * @brief         将通过终端检测按键输入，并控制发送modbus信息
  * @param[in]      none
  * @retval         none
  */
void SmartCarKeyboardTeleopNode::keyboardLoop()
{
    char c;
    bool dirty = false;
    
    /** 
	 * 从终端中获取按键 
	 * int tcgetattr(int fd, struct termios *termios_p);
	 */
    tcgetattr(kfd, &cooked);
    memcpy(&raw, &cooked, sizeof(struct termios));

    /**
	 * c_lflag : 本地模式标志，控制终端编辑功能
	 * ICANON: 使用标准输入模式
	 * ECHO: 显示输入字符
	 */
    raw.c_lflag &=~ (ICANON | ECHO);

	/** 
	 * c_cc[NCCS]：控制字符，用于保存终端驱动程序中的特殊字符，如输入结束符等
	 * VEOL: 附加的End-of-file字符
	 * VEOF: End-of-file字符
	 * */
    raw.c_cc[VEOL] = 1;
    raw.c_cc[VEOF] = 2;
    tcsetattr(kfd, TCSANOW, &raw);
    
    puts("Reading from keyboard");
    puts("Use 1234 keys to control the robot");
    puts("Press Shift to move faster");
    
	/* *
	 * struct pollfd {
　　       int fd;        文件描述符 
　       　short events;  等待的事件 
　　       short revents; 实际发生了的事件 
　       　};
    */
    struct pollfd ufd;
    ufd.fd = kfd;
    ufd.events = POLLIN;
    
    for(;;)
    {
        boost::this_thread::interruption_point();
        
        /* get the next event from the keyboard */
        int num;
        
		/**
		 * poll:把当前的文件指针挂到设备内部定义的等待队列中。
		 * unsigned int (*poll)(struct file * fp, struct poll_table_struct * table)
		 */
        if ((num = poll(&ufd, 1, 250)) < 0)
        {
			/**
			 * perror( ) 用来将上一个函数发生错误的原因输出到标准设备(stderr)。
			 * 参数s所指的字符串会先打印出,后面再加上错误原因字符串。
			 * 此错误原因依照全局变量errno 的值来决定要输出的字符串。
			 * */
            perror("poll():");
            return;
        }
        else if(num > 0)
        {
            if(read(kfd, &c, 1) < 0)
            {
                perror("read():");
                return;
            }
        }
        else
        {
			/* 每按下一次动一下 */
            if (dirty == true)
            {
                dirty = false;
            }
            
            continue;
        }
        ROS_INFO("input:%X",c);
        switch(c)  
        {
            case KEYCODE_1:  
            writeModbus_in(0x01,0x0001,0x0010);//写小车底盘断电
            ROS_INFO("Chassis_off");
            break;  

            case KEYCODE_2:  
            writeModbus_in(0x01,0x0001,0x0001);//写小车底盘上电
            ROS_INFO("Chassis_on");
            break;   

            case KEYCODE_3:  
            writeModbus_in(0x01,0x0002,0x0010);//写路由器断电
            ROS_INFO("Router_on");
            break;  

            case KEYCODE_4:  
            writeModbus_in(0x01,0x0002,0x0001);//写路由器上电
            ROS_INFO("Router_on");
            break;  

            default:
                ROS_INFO("error");

        }


    }
}



